#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
serial::Serial ros_ser;
#define sBUFFERSIZE 4 // send buffer size
unsigned char s_buffer[sBUFFERSIZE];


typedef union
{
    unsigned char Achar[4];
    float Bfloat;
} float_union;
float_union x_pos, y_pos, heading;

void data_to_serial(unsigned char num);
void cmd_to_serial(const geometry_msgs::Twist msg);


void cmd_to_serial(const geometry_msgs::Twist msg)
{
    if(fabs(msg.linear.x) < 1e-6 && fabs(msg.linear.y) < 1e-6 && fabs(msg.angular.z) < 1e-6)
    {
        //stop signal
        data_to_serial('0');
    }
    else if(msg.linear.x > 0)
    {
        //move forward
        data_to_serial('1');
    }
    else if(msg.linear.x < 0)
    {
        //move backward
        data_to_serial('2');
    }
    else if(msg.linear.y > 0)
    {
        //left
        data_to_serial('3');
    }
    else if(msg.linear.y < 0)
    {
        //right
        data_to_serial('4');
    }
    else if(msg.angular.z > 0)
    {
        //left
        data_to_serial('5');
    }
    else if(msg.angular.z < 0)
    {
        //right
        data_to_serial('6');
    }
}

void data_to_serial(unsigned char num)
{
     memset(s_buffer,0,sizeof(s_buffer));
     s_buffer[0]=num;
     s_buffer[1]=0x4c;

     s_buffer[2]=0x5a;
     s_buffer[3]=0x55;

     ros_ser.write(s_buffer,sBUFFERSIZE);
}

void callback(const geometry_msgs::Twist &msg) // callback function
{
    cmd_to_serial(msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_serial_node");
    ros::NodeHandle n;

    ros::Subscriber command_sub = n.subscribe("/turtle1/cmd_vel", 1000, callback);
    ros::Publisher sensor_pub = n.advertise<geometry_msgs::Twist>("/flowerpot/cmd_vel", 10);

    try
    {
        ros_ser.setPort("/dev/ttyUSB0");
        ros_ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ros_ser.setTimeout(to);
        ros_ser.open();
    }

    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if (ros_ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened");
    }
    else
    {
        return -1;
    }
    ros::Rate loop_rate(50);
    while (ros::ok())
    {
        ros::spinOnce();
        size_t n = ros_ser.available();
        if (n != 0)
        {
            unsigned char buffer[15] = {0};
            n = ros_ser.read(buffer, n);
            for (int i = 0; i < n; i++)
            {
                std::cout << std::hex << (buffer[i] & 0xff) << " ";
            }
            std::cout << std::endl;

            if ((buffer[12] & 0xff) == 0x4C && (buffer[13] & 0xff) == 0x5A && (buffer[14] & 0xFF) == 0x55)
            {
                x_pos.Achar[0] = buffer[0];
                x_pos.Achar[1] = buffer[1];
                x_pos.Achar[2] = buffer[2];
                x_pos.Achar[3] = buffer[3];
                y_pos.Achar[0] = buffer[4];
                y_pos.Achar[1] = buffer[5];
                y_pos.Achar[2] = buffer[6];
                y_pos.Achar[3] = buffer[7];
                heading.Achar[0] = buffer[8];
                heading.Achar[1] = buffer[9];
                heading.Achar[2] = buffer[10];
                heading.Achar[3] = buffer[11];
            	std::cout << "x pos: " << x_pos.Bfloat << std::endl;
           	std::cout << "y pos: " << y_pos.Bfloat << std::endl;
           	std::cout << "heading : " << heading.Bfloat << std::endl;
                geometry_msgs::Twist twist;
                twist.linear.x = x_pos.Bfloat;
                twist.linear.y = y_pos.Bfloat;
                twist.angular.z = heading.Bfloat;
                sensor_pub.publish(twist);
            }


        }
        loop_rate.sleep();
    }
}
